
package main 

import (
//	"ry/nbody"
	"ry/mat"
	"cv/highgui"
	"rand"
	"fmt"
)

const maxIters = 100
const goodEnough = 1.0
const c0 = 0.1
const c2 = 0.1
const c3 = 0.1

const winX, winY = 300,300

type particle struct {

	x,y float64
	x_best, y_best float64
	fitness_best float64
	x_neighborhood, y_neighborhood float64
	dx,dy float64
}

// in PSO, this function just determines if 'other' is a neighbor
func (p *particle) React (other *particle) {

	r2 := (p.x - other.x) * (p.x - other.x) + (p.y - other.y) * (p.y - other.y)
	
	if r2 < 100 && other.fitness_best > p.fitness_best {
	
		p.x_neighborhood, p.y_neighborhood = other.x_best, other.y_best
	}
	
}

func (p *particle) Update (dt float64) {

	rx2,ry2 := randParam(), randParam()
	rx3,ry3 := randParam(), randParam()

	p.dx = c0*p.dx + c2*rx2*(p.x_best - p.x) + c3*rx3*(p.x_neighborhood - p.x)
	p.dy = c0*p.dy + c2*ry2*(p.y_best - p.y) + c3*ry3*(p.y_neighborhood - p.y)

	p.x += p.dx * dt
	p.y += p.dy * dt
}

func (p *particle) Init () {


	p.x = rand.Float64 () * 200 + 100
	p.y = rand.Float64 () * 200 + 100
	
	p.x_best, p.y_best = p.x, p.y
}

func (p *particle) fitness () float64 {

	return rand.Float64 ()
}

func randParam () float64 {
	return rand.Float64 () - 0.5
}

func main () {

	fmt.Println ("initializing...")

	particles := [20]particle{}
	
	for i := 0; i < len (particles); i++ {
		particles [i].Init ()
	}
	
	win := highgui.NewWindow("test", 1)
	win.Show(nil, 0)
	img := mat.Zeros (winX, winY)

	fmt.Println ("running")	
	
	for t := 0; t < maxIters; t++ {
		
		for i := 0; i < len (particles); i++ {
		
			for j := 0; j < len (particles); j++ {
				particles [i].React (&particles [j])
			}
	
			particles [i].Update(1.0)
			
			if particles [i].fitness () > goodEnough {
				fmt.Println ("found soln!")
				return
			}
			
			if particles [i].x < 0 {
				particles [i].x = 0
			} else if particles [i].x >= winX {
				particles [i].x = winX - 1
			}
			
			if particles [i].y < 0 {
				particles [i].y = 0
			} else if particles [i].y >= winY {
				particles [i].y = winY - 1
			}
			
			img.Set (int(particles[i].x), int(particles[i].y), 1.0)
		}
		
		win.Show (img, 256.0)
		if highgui.WaitKey (100) != -1 {
			fmt.Println ("canceled.")
			return
		}
		img.Fill (0)
	}
	
	fmt.Println ("ran out of iterations. stopping.")
}
